#ifndef _MPU9250_H_
#define _MPU9250_H_

#include <stdint.h>
#include "platform.h"
#include "board_spi.h"

#ifndef GRAVITY
#	define GRAVITY	9.8015f
#endif

#ifndef PI
#	define PI	3.1415926f
#endif

#define MPUREG_XG_OFFS_TC 0x00
#define MPUREG_YG_OFFS_TC 0x01
#define MPUREG_ZG_OFFS_TC 0x02
#define MPUREG_X_FINE_GAIN 0x03
#define MPUREG_Y_FINE_GAIN 0x04
#define MPUREG_Z_FINE_GAIN 0x05
#define MPUREG_XA_OFFS_H 0x06
#define MPUREG_XA_OFFS_L 0x07
#define MPUREG_YA_OFFS_H 0x08
#define MPUREG_YA_OFFS_L 0x09
#define MPUREG_ZA_OFFS_H 0x0A
#define MPUREG_ZA_OFFS_L 0x0B
#define MPUREG_PRODUCT_ID 0x0C
#define MPUREG_SELF_TEST_X 0x0D
#define MPUREG_SELF_TEST_Y 0x0E
#define MPUREG_SELF_TEST_Z 0x0F
#define MPUREG_SELF_TEST_A 0x10
#define MPUREG_XG_OFFS_USRH 0x13
#define MPUREG_XG_OFFS_USRL 0x14
#define MPUREG_YG_OFFS_USRH 0x15
#define MPUREG_YG_OFFS_USRL 0x16
#define MPUREG_ZG_OFFS_USRH 0x17
#define MPUREG_ZG_OFFS_USRL 0x18
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_ACCEL_CONFIG_2      0x1D
#define MPUREG_LP_ACCEL_ODR        0x1E
#define MPUREG_MOT_THR             0x1F
#define MPUREG_FIFO_EN             0x23
#define MPUREG_I2C_MST_CTRL        0x24
#define MPUREG_I2C_SLV0_ADDR       0x25
#define MPUREG_I2C_SLV0_REG        0x26
#define MPUREG_I2C_SLV0_CTRL       0x27
#define MPUREG_I2C_SLV1_ADDR       0x28
#define MPUREG_I2C_SLV1_REG        0x29
#define MPUREG_I2C_SLV1_CTRL       0x2A
#define MPUREG_I2C_SLV2_ADDR       0x2B
#define MPUREG_I2C_SLV2_REG        0x2C
#define MPUREG_I2C_SLV2_CTRL       0x2D
#define MPUREG_I2C_SLV3_ADDR       0x2E
#define MPUREG_I2C_SLV3_REG        0x2F
#define MPUREG_I2C_SLV3_CTRL       0x30
#define MPUREG_I2C_SLV4_ADDR       0x31
#define MPUREG_I2C_SLV4_REG        0x32
#define MPUREG_I2C_SLV4_DO         0x33
#define MPUREG_I2C_SLV4_CTRL       0x34
#define MPUREG_I2C_SLV4_DI         0x35
#define MPUREG_I2C_MST_STATUS      0x36
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_EXT_SENS_DATA_00    0x49
#define MPUREG_EXT_SENS_DATA_01    0x4A
#define MPUREG_EXT_SENS_DATA_02    0x4B
#define MPUREG_EXT_SENS_DATA_03    0x4C
#define MPUREG_EXT_SENS_DATA_04    0x4D
#define MPUREG_EXT_SENS_DATA_05    0x4E
#define MPUREG_EXT_SENS_DATA_06    0x4F
#define MPUREG_EXT_SENS_DATA_07    0x50
#define MPUREG_EXT_SENS_DATA_08    0x51
#define MPUREG_EXT_SENS_DATA_09    0x52
#define MPUREG_EXT_SENS_DATA_10    0x53
#define MPUREG_EXT_SENS_DATA_11    0x54
#define MPUREG_EXT_SENS_DATA_12    0x55
#define MPUREG_EXT_SENS_DATA_13    0x56
#define MPUREG_EXT_SENS_DATA_14    0x57
#define MPUREG_EXT_SENS_DATA_15    0x58
#define MPUREG_EXT_SENS_DATA_16    0x59
#define MPUREG_EXT_SENS_DATA_17    0x5A
#define MPUREG_EXT_SENS_DATA_18    0x5B
#define MPUREG_EXT_SENS_DATA_19    0x5C
#define MPUREG_EXT_SENS_DATA_20    0x5D
#define MPUREG_EXT_SENS_DATA_21    0x5E
#define MPUREG_EXT_SENS_DATA_22    0x5F
#define MPUREG_EXT_SENS_DATA_23    0x60
#define MPUREG_I2C_SLV0_DO         0x63
#define MPUREG_I2C_SLV1_DO         0x64
#define MPUREG_I2C_SLV2_DO         0x65
#define MPUREG_I2C_SLV3_DO         0x66
#define MPUREG_I2C_MST_DELAY_CTRL  0x67
#define MPUREG_SIGNAL_PATH_RESET   0x68
#define MPUREG_MOT_DETECT_CTRL     0x69
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_BANK_SEL 0x6D
#define MPUREG_MEM_START_ADDR 0x6E
#define MPUREG_MEM_R_W 0x6F
#define MPUREG_DMP_CFG_1 0x70
#define MPUREG_DMP_CFG_2 0x71
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
#define MPUREG_WHOAMI 0x75
#define MPUREG_XA_OFFSET_H         0x77
#define MPUREG_XA_OFFSET_L         0x78
#define MPUREG_YA_OFFSET_H         0x7A
#define MPUREG_YA_OFFSET_L         0x7B
#define MPUREG_ZA_OFFSET_H         0x7D
#define MPUREG_ZA_OFFSET_L         0x7E
/* ---- AK8963 Reg In MPU9250 ----------------------------------------------- */

#define AK8963_I2C_ADDR             0x0C//0x18
#define AK8963_Device_ID            0x48

// Read-only Reg
#define AK8963_WIA                  0x00
#define AK8963_INFO                 0x01
#define AK8963_ST1                  0x02
#define AK8963_HXL                  0x03
#define AK8963_HXH                  0x04
#define AK8963_HYL                  0x05
#define AK8963_HYH                  0x06
#define AK8963_HZL                  0x07
#define AK8963_HZH                  0x08
#define AK8963_ST2                  0x09
// Write/Read Reg
#define AK8963_CNTL1                0x0A
#define AK8963_CNTL2                0x0B
#define AK8963_ASTC                 0x0C
#define AK8963_TS1                  0x0D
#define AK8963_TS2                  0x0E
#define AK8963_I2CDIS               0x0F
// Read-only Reg ( ROM )
#define AK8963_ASAX                 0x10
#define AK8963_ASAY                 0x11
#define AK8963_ASAZ                 0x12

// Configuration bits mpu9250
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_FS_250DPS              0x00
#define BITS_FS_500DPS              0x08
#define BITS_FS_1000DPS             0x10
#define BITS_FS_2000DPS             0x18
#define BITS_FS_2G                  0x00
#define BITS_FS_4G                  0x08
#define BITS_FS_8G                  0x10
#define BITS_FS_16G                 0x18
#define BITS_FS_MASK                0x18
#define BITS_DLPF_CFG_256HZ_NOLPF2  0x00
#define BITS_DLPF_CFG_188HZ         0x01
#define BITS_DLPF_CFG_98HZ          0x02
#define BITS_DLPF_CFG_42HZ          0x03
#define BITS_DLPF_CFG_20HZ          0x04
#define BITS_DLPF_CFG_10HZ          0x05
#define BITS_DLPF_CFG_5HZ           0x06
#define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
#define BITS_DLPF_CFG_MASK          0x07
#define BIT_INT_ANYRD_2CLEAR        0x10
#define BIT_RAW_RDY_EN              0x01
#define BIT_I2C_IF_DIS              0x10

#define READ_FLAG   0x80

/* ---- Sensitivity --------------------------------------------------------- */

#define MPU9250A_2g       ((float)0.000061035156f * GRAVITY * 1000) // 0.000061035156 g/LSB --> 0.001m/s^2/LSB
#define MPU9250A_4g       ((float)0.000122070312f * GRAVITY * 1000) // 0.000122070312 g/LSB --> 0.001m/s^2/LSB
#define MPU9250A_8g       ((float)0.000244140625f * GRAVITY * 1000) // 0.000244140625 g/LSB --> 0.001m/s^2/LSB
#define MPU9250A_16g      ((float)0.000488281250f * GRAVITY * 1000) // 0.000488281250 g/LSB --> 0.001m/s^2/LSB

#define MPU9250G_250dps   ((float)0.007633587786f / 180 * PI * 500) // 0.007633587786 dps/LSB --> 0.002rad/s/LSB
#define MPU9250G_500dps   ((float)0.015267175572f / 180 * PI * 500) // 0.015267175572 dps/LSB --> 0.002rad/s/LSB
#define MPU9250G_1000dps  ((float)0.030487804878f / 180 * PI * 500) // 0.030487804878 dps/LSB --> 0.002rad/s/LSB
#define MPU9250G_2000dps  ((float)0.060975609756f / 180 * PI * 500) // 0.060975609756 dps/LSB --> 0.002rad/s/LSB

#define MPU9250M_4800uT   ((float)0.6f * 10)            // 0.6 uT/LSB --> 0.1uT/LSB

#define MPU9250T_85degC   ((float)0.002995177763f * 10) // 0.002995177763 degC/LSB --> 0.1degC/LSB

typedef struct {
#define MPU9250_CLK_BITMASK	(uint8_t)0x07
#define MPU9250_CLK_INTERNAL	(uint8_t)0x00
#define MPU9250_CLK_AUTO	(uint8_t)0x01
#define MPU9250_CLK_STOP	(uint8_t)0x07
	uint8_t clk_sel;
#define MPU9250_GLPF_BW_250HZ		0x00
#define MPU9250_GLPF_BW_184HZ         	0x01
#define MPU9250_GLPF_BW_92HZ          	0x02
#define MPU9250_GLPF_BW_41HZ          	0x03
#define MPU9250_GLPF_BW_20HZ          	0x04
#define MPU9250_GLPF_BW_10HZ          	0x05
#define MPU9250_GLPF_BW_5HZ           	0x06
	uint8_t gyr_lpf_bw;
#define MPU9250_GFS_BITMASK	(uint8_t)0x18
#define MPU9250_GFS_250		((uint8_t)0x00 << 3)
#define MPU9250_GFS_500		((uint8_t)0x01 << 3)
#define MPU9250_GFS_1000	((uint8_t)0x02 << 3)
#define MPU9250_GFS_2000	((uint8_t)0x03 << 3)
	uint8_t gyr_fs;
#define MPU9250_ALPF_BW_1130	(uint8_t)0x00
#define MPU9250_ALPF_BW_460	(uint8_t)0x00
#define MPU9250_ALPF_BW_184	(uint8_t)0x01
#define MPU9250_ALPF_BW_92	(uint8_t)0x02
#define MPU9250_ALPF_BW_41	(uint8_t)0x03
#define MPU9250_ALPF_BW_20	(uint8_t)0x04
#define MPU9250_ALPF_BW_10	(uint8_t)0x05
#define MPU9250_ALPF_BW_5	(uint8_t)0x06
	uint8_t acc_lpf_bw;
#define MPU9250_AFS_BITMASK	(uint8_t)0x18
#define MPU9250_AFS_2G		((uint8_t)0x00 << 3)
#define MPU9250_AFS_4G		((uint8_t)0x01 << 3)
#define MPU9250_AFS_8G		((uint8_t)0x02 << 3)
#define MPU9250_AFS_16G		((uint8_t)0x03 << 3)
	uint8_t acc_fs;
	uint8_t status;
} MPU9250_InitTypeDef;

typedef struct {
	struct imu_raw_data imu;
	struct mag_raw_data mag;
	int16_t temp;
} mpu9250_data_t;

/* working status enum */
enum {
	INIT = 0,
	CHECK_FS,
	UPDATE_MAG_SENS,
	READ_MAG_SENS,
	READ_IMU,
	UPDATE_MAG_DATA,
	READ_ALL,
	SET_GFS,
	SET_AFS
};

/* init status enum */
enum {
	INIT_RESET = 0,
	INIT_CLK_SEL,
	INIT_ENALE_SENSOR,
	INIT_GLPF,
	INIT_GFS,
	INIT_AFS,
	INIT_ALPF,
	INIT_INTCFG,
	INIT_I2C_MASTER,
	INIT_I2C_RATE,
	INIT_MAG_I2C_ADDR,
	INIT_MAG_RESET_ADDR,
	INIT_MAG_RESET_DATA,
	INIT_MAG_SEND_RESET,
	INIT_MAG_CTRL_ADDR,
	INIT_MAG_CTRL_DATA,
	INIT_MAG_SEND_CTRL,
	INIT_COMPLETE
};

typedef struct {
	SPI_HandleTypeDef* spi;
	MPU9250_InitTypeDef initStructure;
	uint8_t* rx_byte;
	mpu9250_data_t	raw_data;
	uint8_t readMagAfterNImu;
	uint8_t init_delay;
	float magSensAdj[3];
	void (*rx_handler)(void* pMpu);
	volatile uint8_t status;
} MPU9250_HandleTypeDef;

void mpu9250_nonblocking_process(MPU9250_HandleTypeDef* mpuHandler);

uint8_t mpu9250_set_acc_range(uint8_t range);
uint8_t mpu9250_set_gyro_range(uint8_t range);

/* test function */
uint8_t mpu9250_read_pid(MPU9250_HandleTypeDef* mpu);
uint8_t mpu9250_read_ak8963_pid(MPU9250_HandleTypeDef* mpu);

#endif
